#include <Arduino.h>
#include <U8g2lib.h>
#include <math.h>
#include <dummy.h>
#include <WiFi.h>
#include <esp32servo.h>


#define PI 3.1415926535897932384626433832795
#define val PI / 180
#define max 320//扫描范围
#define speed 2//扫描速度
#define times 2//每个点采样次数
Servo myservo;
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ 36, /* data=*/ 37);   // All Boards without Reset of the Display
hw_timer_t *tim1 = NULL;
float siny[181];//角度数据
float cosx[181];
int pos[181];//位置数据
int is=0;
int tim1_IRQ_count = 0;
int manydis(int n);
//111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111

#define server_ip "bemfa.com" 
#define server_port "8344" 

#define wifi_name  "wifi名称"     
#define wifi_password   "wifi密码"  
String UID = "巴法云的唯一标识"; 
String TOPIC = "tcp设备名称";         
const int LED_Pin = 0;              
const int LED_Pin2 = 1;

String my_led_status = "off";

#define MAX_PACKETSIZE 512
//心跳值60s
#define upDataTime 1.5*1000

WiFiClient TCPclient;
String TcpClient_Buff = "";//初始化字符串
unsigned int TcpClient_BuffIndex = 0;
unsigned long TcpClient_preTick = 0;
unsigned long preHeartTick = 0;
unsigned long preTCPStartTick = 0;//连接
bool preTCPConnected = false;
//相关函数初始化
//连接WIFI
void doWiFiTick();
void startSTA();

//TCP初始化连接
void doTCPClientTick();
void startTCPClient();
void sendtoTCPServer(String p);


void turnOnLed();
void turnOffLed();


void sendtoTCPServer(String p){
  if (!TCPclient.connected()) 
  {
    Serial.println("Client is not readly");
    return;
  }
  TCPclient.print(p);
  preHeartTick = millis();
}


void startTCPClient(){
  if(TCPclient.connect(server_ip, atoi(server_port))){
    Serial.print("\nConnected to server:");
    Serial.printf("%s:%d\r\n",server_ip,atoi(server_port));
    
    String tcpTemp=""; 
    tcpTemp = "cmd=1&uid="+UID+"&topic="+TOPIC+"\r\n"; 
    sendtoTCPServer(tcpTemp);
    tcpTemp="";

    
    preTCPConnected = true;
    TCPclient.setNoDelay(true);
  }
  else{
    Serial.print("Failed connected to server:");
    Serial.println(server_ip);
    TCPclient.stop();
    preTCPConnected = false;
  }
  preTCPStartTick = millis();
}

void doTCPClientTick(){

   if(WiFi.status() != WL_CONNECTED) return;
  if (!TCPclient.connected()) {
  if(preTCPConnected == true){
    preTCPConnected = false;
    preTCPStartTick = millis();
    Serial.println();
    Serial.println("TCP Client disconnected.");
    TCPclient.stop();
  }
  else if(millis() - preTCPStartTick > 1*1000)
    TCPclient.stop();
    startTCPClient();
  }
  else
  {
    if (TCPclient.available()) {
      char c =TCPclient.read();
      TcpClient_Buff +=c;
      TcpClient_BuffIndex++;
      TcpClient_preTick = millis();
      
      if(TcpClient_BuffIndex>=MAX_PACKETSIZE - 1){
        TcpClient_BuffIndex = MAX_PACKETSIZE-2;
        TcpClient_preTick = TcpClient_preTick - 200;
      }
 
    }
    if(millis() - preHeartTick >= upDataTime){
      preHeartTick = millis();
       /*****************获取数据*****************/
        int ssa =0;
        ssa = manydis(2);

      String upstr = "";
      upstr = "cmd=2&uid="+UID+"&topic="+TOPIC+"&msg=#"+ssa+"#"+is+"#"+my_led_status+"#\r\n";
      sendtoTCPServer(upstr);
      upstr = "";
    }
  }
  if((TcpClient_Buff.length() >= 1) && (millis() - TcpClient_preTick>=200))
  {
    //data ready
    TCPclient.flush();
    Serial.println("Buff");
    Serial.println(TcpClient_Buff);
    //////字符串匹配，检测发了的字符串TcpClient_Buff里面是否包含&msg=on，如果有，则打开开关
    if((TcpClient_Buff.indexOf("&msg=on") > 0)) {
      turnOnLed();
    //////字符串匹配，检测发了的字符串TcpClient_Buff里面是否包含&msg=off，如果有，则关闭开关
    }else if((TcpClient_Buff.indexOf("&msg=off") > 0)) {
      turnOffLed();
    }
     TcpClient_Buff="";//清空字符串，以便下次接收
     TcpClient_BuffIndex = 0;
   }
}

//打开灯泡
void turnOnLed(){
  Serial.println("Turn ON");
  digitalWrite(LED_Pin,LOW);
  my_led_status = "on";
}
//关闭灯泡
void turnOffLed(){
  Serial.println("Turn OFF");
  digitalWrite(LED_Pin,HIGH);
  my_led_status  = "off";
}
void startSTA(){
  WiFi.disconnect();
  WiFi.mode(WIFI_STA);
  WiFi.begin(wifi_name, wifi_password);
}


void doWiFiTick(){
  static bool startSTAFlag = false;
  static bool taskStarted = false;
  static uint32_t lastWiFiCheckTick = 0;

  if (!startSTAFlag) {
    startSTAFlag = true;
    startSTA();
  }


  if ( WiFi.status() != WL_CONNECTED ) {
    if (millis() - lastWiFiCheckTick > 1000) {
      lastWiFiCheckTick = millis();
    }
  }
  
  else {
    if (taskStarted == false) {
      taskStarted = true;
      Serial.print("\r\nGet IP Address: ");
      Serial.println(WiFi.localIP());
      startTCPClient();
    }
  }
}
//000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
void tim1Interrupt()
{//中断服务函数
  tim1_IRQ_count++;
}
float getdistance(){
  float distance,distance_mm;
  int time_end;
	digitalWrite(15,HIGH);						
	delayMicroseconds(15);									
	digitalWrite(15,LOW);						
	tim1_IRQ_count=0;
	while(digitalRead(2)==0){
	if(tim1_IRQ_count>800)
		break;
	}		
	tim1_IRQ_count=0;												//计时清零
	while(digitalRead(2)==1);		
  
	time_end=tim1_IRQ_count;										

	if(time_end/100<38&&time_end>0)								
	{
		distance=(time_end*346)/2;					
		distance_mm=distance/100;						
	}
	return distance_mm;									//返回测距结果
}
void setup(void) {
  u8g2.begin();
  for(int i=0;i<=180;i++){
    siny[i]=sin(float(i)*val);
    cosx[i]=cos(float(i)*val);
  }
  tim1 = timerBegin(0, 80, true);
  timerAttachInterrupt(tim1, tim1Interrupt, true);//更新中断
  timerAlarmWrite(tim1, 10, true);//计数次数10
  timerAlarmEnable(tim1);//开启定时器
  myservo.attach(16);//绑定io16
  pinMode(15,OUTPUT);
  digitalWrite(15,HIGH);
  pinMode(4,OUTPUT);
  digitalWrite(4,LOW);

  Serial.begin(115200);
  pinMode(LED_Pin,OUTPUT);
  pinMode(LED_Pin2,OUTPUT);
  digitalWrite(LED_Pin,LOW);
  digitalWrite(LED_Pin2,HIGH);
  Serial.println("Beginning...");

}

int manydis(int n)//测量取平均值
{ float distancen[n];
  float alldistance;
  int distance;
  for(int k=0;k<n;k++){
      distancen[k]=getdistance();
      if(distancen[k]==0)
        n--;
      delay(5);
      alldistance+=distancen[k];
    }
    if(n==0)
      distance=max;
    else
      distance=int(alldistance)/n;
      if(distance>max)
          distance=max;
    return distance;
}
void showleftpos(int distance,int i){
    int x=64+cosx[i]*(64);
    int y=64-siny[i]*(64);
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_helvB08_tf);
    for(int j=0;j<i;j++){
      u8g2.drawPixel(64+cosx[j]*(pos[j]/(max/64)),64-siny[j]*(pos[j]/(max/64)));
    }
    u8g2.drawLine(64,64,x,y);
    u8g2.setCursor(0,10);
    u8g2.print(distance);
    u8g2.setCursor(110,10);
    u8g2.print(i);
    u8g2.sendBuffer();
}
void showrightpos(int distance,int i){
    int x=64+cosx[i]*(64);
    int y=64-siny[i]*(64);
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_helvB08_tf);
    for(int j=180;j>i;j--){
      u8g2.drawPixel(64+cosx[j]*(pos[j]/(max/64)),64-siny[j]*(pos[j]/(max/64)));
    }
    u8g2.drawLine(64,64,x,y);
    u8g2.setCursor(0,10);
    u8g2.print(distance);
    u8g2.setCursor(110,10);
    u8g2.print(i);
    u8g2.sendBuffer();
}
void loop(void) {

  doWiFiTick();
  u8g2.clearBuffer();
  int distance;
  for(int i=0;i<181;i+=speed)
  { 
    is=i;
    myservo.write(i);
    distance=manydis(times);
    pos[i]=distance;
    showleftpos(distance,i);
    doTCPClientTick();
  }
  for(int i=180;i>0;i-=speed)
  { 
    is=i;
    myservo.write(i);
    distance=manydis(times);
    pos[i]=distance;
    showrightpos(distance,i);
    doTCPClientTick();
  }
    
    
}
